#include "gesturewalktest.h"
#include <statechangeevent.h>
#include <handsmoveevent.h>
#include <handgestureevent.h>

#include <cv.h>
#include <iostream>
#include <igraphic/image.h>
#include <ivideo/txtmgr.h>
#include <ivideo/texture.h>
#include <cstool/cspixmap.h>
#include <imap/loader.h>
#include <cstool/genmeshbuilder.h>

#include <fstream>
#include <string>
#include <boost/lexical_cast.hpp>

#include "crystalspace.h"

using namespace std;
using namespace mlgesture;
using namespace cv;
using namespace boost;


GestureWalkTest::GestureWalkTest() : WalkTest() {
	gesture_ = NULL;
	txtmgr_ = NULL;
	gestureState_ = false;
	newInit_ = true;
	goForward_ = false;
	goBackward_ = false;
	weaponNr_ = 0;
	isShooting_ = false;
	nextShoot_ = 0;
}

GestureWalkTest::~GestureWalkTest() {
	vector<csPixmap*>::iterator it = guns_.begin();
	while (it != guns_.end()) {
		delete (*it);
		it = guns_.erase(it);
	}
	for (int i=0; i<10; i++) {
		delete scoreTex_[i];
	}
	vector<Bullet*>::iterator it_bul = bullets_.begin();
	while (it_bul != bullets_.end()) {
		it_bul = bullets_.erase(it_bul);
	}
	delete bulletIcon_;

	if (gesture_) {
		MLGesture::removeObserver(this);
		gesture_->stopProcess();
		gesture_->releaseCamera();
		delete gesture_;
	}
}

void GestureWalkTest::processEvent(GestureEvent *event) {
	StateChangeEvent *state_event;
	HandsMoveEvent *move_event;
	HandGestureEvent *static_event, *dynamic_event;

	vector<Point> centers;
	vector<Rect> hands_rect;
	float rotate_speed = 3.0f;
	Size frame_size = gesture_->getFrameSize();

	switch(event->getType()) {
	case GestureEvent::STATE_EVENT:
		state_event = dynamic_cast<StateChangeEvent*>(event);
		gestureState_ = state_event->getState();
		if (!gestureState_) {
			Step(0);
		}
		//newInit_ = true;
		break;
	case GestureEvent::MOVE_EVENT:
		move_event = dynamic_cast<HandsMoveEvent*>(event);
		move_event->getCenters(centers);
		move_event->getRects(hands_rect);
		if (newInit_) {
			lastLx = centers[0].x;
			lastLy = centers[0].y;
			//RotateCam(-rotate_speed*(-frame_size.height/2 + lastLy),0);
			newInit_ = false;
		}
		else {
			int dx = centers[0].x - lastLx;
			int dy = centers[0].y - lastLy;
			float vy = (-0.4*dy/(frame_size.height-hands_rect[0].height));
			float vx = (0.6*dx/(frame_size.width/2));
			if (hands_rect[0].x < 5) {
				vx = -0.03;
			}
			else if (hands_rect[0].x + hands_rect[0].width > frame_size.width/2 - 0.1 * frame_size.width) {
				vx = 0.03;
			}
			if (hands_rect[0].y < 2) {
				vy = 0.03;
			}
			else if (hands_rect[0].y + hands_rect[0].height > frame_size.height - 2) {
				vy = -0.03;
			}
			RotateCam (
				  rotate_speed * vy,
				  rotate_speed * vx);
			lastLx = centers[0].x;
			lastLy = centers[0].y;
			csVector3 rot = collider_actor.GetRotation();
			if (rot.x < -1.57) {
				rot.x = -1.57;
				collider_actor.SetRotation(rot);
			}
			else if (rot.x > 1.57) {
				rot.x = 1.57;
				collider_actor.SetRotation(rot);
			}
		}
		break;
	case GestureEvent::STATIC_BEGIN_EVENT:
		static_event = dynamic_cast<HandGestureEvent*>(event);
		if (static_event->getHandId() == 0) {
			if (static_event->getGestureId() == 3) {
				Step(-0.7);
			}
			else if (static_event->getGestureId() == 2) {
				Step(0.7);
			}
		}
		else if (static_event->getHandId() == 1) {
			if (static_event->getGestureId() == 2) {
				isShooting_ = true;
			}
		}
		break;
	case GestureEvent::STATIC_END_EVENT:
		static_event = dynamic_cast<HandGestureEvent*>(event);
		if (static_event->getHandId() == 0) {
			if (static_event->getGestureId() == 3) {
				Step(0);
			}
			else if (static_event->getGestureId() == 2) {
				Step(0);
			}
		}
		else if (static_event->getHandId() == 1) {
			if (static_event->getGestureId() == 2) {
				isShooting_ = false;
			}
		}
		break;
	case GestureEvent::DYNAMIC_EVENT:
		dynamic_event = dynamic_cast<HandGestureEvent*>(event);
		if (dynamic_event->getHandId() == 1) {
			if (dynamic_event->getGestureId() == 8) {
				Jump();
			}
			else if (dynamic_event->getGestureId() == 1) {
				weaponNr_++;
			}
			else if (dynamic_event->getGestureId() == 0) {
				weaponNr_--;
			}
			else if (dynamic_event->getGestureId() == 6) {
				gunsAmmo[weaponNr_] = maxGunsAmmo[weaponNr_];
			}
			if (weaponNr_ < 0) weaponNr_ = 2;
			else if (weaponNr_ > 2) weaponNr_ = 0;
			nextShoot_ = bulletSpeed[weaponNr_];
		}
		break;
	default:
		break;
	}
}

bool GestureWalkTest::Initialize(int argc, const char *const argv[], const char *iConfigName) {
	if (!initGesture()) {
		return false;
	}
	if (!WalkTest::Initialize(argc, argv, iConfigName))
			return false;
	iNativeWindow* nw = myG2D->GetNativeWindow ();
	if (nw) nw->SetTitle ("Gesture FPS");
	return true;
}

void GestureWalkTest::DrawFrame2D() {
	if (!txtmgr_) {
		txtmgr_ = myG3D->GetTextureManager();

		iTextureWrapper* txt = LevelLoader->LoadTexture ("gun1", "/gesture/gun1.png", CS_TEXTURE_2D, txtmgr_, true);
		iTextureHandle* handle = txt->GetTextureHandle();
		guns_.push_back(new csSimplePixmap(handle));
		gunsAmmo.push_back(50);
		maxGunsAmmo.push_back(50);
		bulletSpeed.push_back(300);
		txt = LevelLoader->LoadTexture ("gun2", "/gesture/gun2.png", CS_TEXTURE_2D, txtmgr_, true);
		handle = txt->GetTextureHandle();
		guns_.push_back(new csSimplePixmap(handle));
		gunsAmmo.push_back(25);
		maxGunsAmmo.push_back(25);
		bulletSpeed.push_back(500);
		txt = LevelLoader->LoadTexture ("gun3", "/gesture/gun3.png", CS_TEXTURE_2D, txtmgr_, true);
		handle = txt->GetTextureHandle();
		guns_.push_back(new csSimplePixmap(handle));
		gunsAmmo.push_back(8);
		maxGunsAmmo.push_back(8);
		bulletSpeed.push_back(1000);

		txt = LevelLoader->LoadTexture ("bulletIcon", "/gesture/bulleticon.png", CS_TEXTURE_2D, txtmgr_, true);
		handle = txt->GetTextureHandle();
		bulletIcon_ = new csSimplePixmap(handle);

		csRef<iMeshFactoryWrapper> fact = Engine->CreateMeshFactory (
		  "crystalspace.mesh.object.genmesh", "bulletFact");
		csRef<iGeneralFactoryState> factstate = scfQueryInterface<iGeneralFactoryState> (
		  fact->GetMeshObjectFactory ());

		csEllipsoid ellips (csVector3 (0, 0, 0), csVector3 (0.008,0.008,0.008));
		factstate->GenerateSphere (ellips, 8);
		factstate->CalculateNormals ();

		LevelLoader->LoadTexture ("stone", "/lib/std/hazebasic.png");
		iMaterialWrapper* tm = Engine->GetMaterialList ()->FindByName ("stone");
		fact->GetMeshObjectFactory ()-> SetMaterialWrapper (tm);

		loadScoreTexture();
	}
	guns_[weaponNr_]->DrawScaled(myG3D,FrameWidth-185,FrameHeight-165,180,160);

	Mat frame, frame2, mask;
	gesture_->getFrame(frame);
	if (frame.empty()) return;
	resize(frame,frame2,Size(128,128));
	vector<Mat> channels;
	cv::split(frame2, channels);
	Mat alpha = Mat::ones(frame2.size(),CV_8UC1)*255;
	channels.push_back(alpha);
	cv::merge(channels, frame);
	csRef<iTextureHandle> texture = txtmgr_->CreateTexture(128,128,csimg2D, "rgba8", CS_TEXTURE_2D);
	if (!texture) {
		return;
	}
	texture->Blit(0,0,128,128,(unsigned char const*)(frame.data),iTextureHandle::BGRA8888);
	csPixmap *pixmap = new csSimplePixmap(texture);
	pixmap->DrawScaled(myG3D,5,5,160,120);
	delete pixmap;

	if (!gestureState_) {
		gesture_->getMask(mask);
		if (mask.empty()) return;
		resize(mask,frame2,Size(128,128));
		cv::split(frame2, channels);
		channels.push_back(alpha);
		cv::merge(channels, frame);
		texture = txtmgr_->CreateTexture(128,128,csimg2D, "rgba8", CS_TEXTURE_2D);
		if (!texture) {
			return;
		}
		texture->Blit(0,0,128,128,(unsigned char const*)(frame.data),iTextureHandle::BGRA8888);
		csPixmap *pixmap2 = new csSimplePixmap(texture);
		pixmap2->DrawScaled(myG3D,FrameWidth-165,5,160,120);
		delete pixmap2;
	}

	int posX = 60;
	int num = gunsAmmo[weaponNr_];
	int cyfraPos = 0;
	while (num > 0 || (num==0 && cyfraPos==0)) {
		int cyfra = num%10;
		scoreTex_[cyfra]->DrawScaled(myG3D,FrameWidth-posX,FrameHeight-190,20,30);
		num /= 10;
		cyfraPos++;
		posX += 25;
	}
	bulletIcon_->DrawScaled(myG3D,FrameWidth-140,FrameHeight-188,25,25);
}

void GestureWalkTest::MoveSystems(csTicks elapsed_time, csTicks current_time) {
	if (nextShoot_>0) {
		nextShoot_ -= elapsed_time;
	}
	if (isShooting_)
		shoot();
	WalkTest::MoveSystems(elapsed_time, current_time);
	for (vector<Bullet*>::iterator it = bullets_.begin(); it != bullets_.end();) {
		csVector3 old_p = (*it)->mesh_->GetMovable()->GetPosition ();
		csVector3 new_p = old_p + 0.4 * (*it)->dir_;
		(*it)->mesh_->GetMovable()->SetPosition (new_p);
		(*it)->mesh_->GetMovable()->UpdateMove();
		(*it)->distance_ += 0.4;
		if ((*it)->distance_ > 10) {
			Engine->WantToDie((*it)->mesh_);
			delete *it;
			it = bullets_.erase(it);
		}
		else {
			++it;
		}
	}
}

void GestureWalkTest::LoadModels() {

}

void GestureWalkTest::shoot() {
	if (nextShoot_ > 0 || gunsAmmo[weaponNr_] == 0) return;
	nextShoot_ = bulletSpeed[weaponNr_];
	gunsAmmo[weaponNr_]--;

	csVector3 rot = collider_actor.GetRotation();
	csVector3 rot_vec(sin(rot.y)*cos(rot.x),sin(rot.x),cos(rot.y)*cos(rot.x));
	csVector3 dir (0,0,0);
	csVector3 v = Sys->view->GetCamera ()->GetTransform ().This2Other (dir);
	bullets_.push_back(new Bullet(v, rot_vec, view->GetCamera()->GetSector()));
}

void GestureWalkTest::loadScoreTexture() {
	iTextureWrapper* txt = LevelLoader->LoadTexture ("score0", "/gesture/score0.png", CS_TEXTURE_2D, txtmgr_, true);
	iTextureHandle* handle = txt->GetTextureHandle();
	scoreTex_[0] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score1", "/gesture/score1.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[1] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score2", "/gesture/score2.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[2] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score3", "/gesture/score3.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[3] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score4", "/gesture/score4.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[4] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score5", "/gesture/score5.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[5] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score6", "/gesture/score6.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[6] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score7", "/gesture/score7.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[7] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score8", "/gesture/score8.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[8] = new csSimplePixmap(handle);
	txt = LevelLoader->LoadTexture ("score9", "/gesture/score9.png", CS_TEXTURE_2D, txtmgr_, true);
	handle = txt->GetTextureHandle();
	scoreTex_[9] = new csSimplePixmap(handle);
}

bool GestureWalkTest::readCfg() {
	camId_ = -1;
	lowHue_ = 179;
	upperHue_ = 45;
	lowValue_ = 70;
	upperValue_ = 255;
	trackingSensity_ = 92;
	fstream f("demo.cfg");
	bool state = true;
	string line;
	if (!f.is_open()) {
		return false;
	}
	getline(f, line);
	while (f.good()){
		size_t pos = line.find_first_of(":");
		if (pos == string::npos) {
			state = false;
			break;
		}
		string cmd = line.substr(0, pos);
		string value = line.substr(pos+1);
		int ivalue;
		try {
			ivalue = lexical_cast<int>(value);
		}
		catch (bad_lexical_cast&) {
			state = false;
			break;
		}
		if (cmd.compare("camera_id")==0) {
			camId_ = ivalue;
		}
		else if (cmd.compare("low_hue")==0) {
			if (ivalue < 0 || ivalue > 359) {
				state = false;
				break;
			}
			lowHue_ = ivalue;
		}
		else if (cmd.compare("upper_hue")==0) {
			if (ivalue < 0 || ivalue > 359) {
				state = false;
				break;
			}
			upperHue_ = ivalue;
		}
		else if (cmd.compare("low_value")==0) {
			if (ivalue < 0 || ivalue > 255) {
				state = false;
				break;
			}
			lowValue_ = ivalue;
		}
		else if (cmd.compare("upper_value")==0) {
			if (ivalue < 0 || ivalue > 255) {
				state = false;
				break;
			}
			upperValue_ = ivalue;
		}
		else if (cmd.compare("tracking_sensity")==0) {
			if (ivalue < 0 || ivalue > 100) {
				state = false;
				break;
			}
			trackingSensity_ = ivalue;
		}
		getline(f, line);
	}

	f.close();
	return state;
}

bool GestureWalkTest::initGesture() {
	if (!readCfg())
		return false;

	gesture_ = new MLGesture(camId_, 2);

	gesture_->adjustSkinFilter(lowHue_, upperHue_, lowValue_, upperValue_);
	gesture_->adjustTrackingSensity(1.*trackingSensity_/100);
	gesture_->setAdaptiveAlgoritm(false);
	gesture_->enableStaticGesture(true);
	gesture_->loadStaticGesturesModels("models/static.dat.model", "models/static.dat.scale_factors", "models/label.dat");

	gesture_->enableTrajectory(true);
	gesture_->enableDynamicGesture(true);

	models.push_back("models/circle_l_model.xml");
	models.push_back("models/circle_r_model.xml");
	models.push_back("models/down_model.xml");
	models.push_back("models/up_model.xml");
	models.push_back("models/left_model.xml");
	models.push_back("models/right_model.xml");
	models.push_back("models/right_left_model.xml");
	models.push_back("models/left_right_model.xml");
	models.push_back("models/up_down_model.xml");
	models.push_back("models/down_up_model.xml");
	gesture_->loadDynamicGestureModels(models, "models/garbage_model.xml");
	MLGesture::addObserver(this);
	gesture_->startProcess();

	return true;
}
